#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <px4flow/Flow.h>
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <nav_msgs/Odometry.h>
#include <string>
#include <string.h>
#include <sstream>
#include <iostream>
#include <fstream> 
#include <vector>
#include <stdio.h>

// [need to install serial pkg first!!!!!] sudo apt-get install ros-kinetic-serial
#define  BYTE0(dwTemp)       ( *( (uint8_t *)(&dwTemp)	)  )
#define  BYTE1(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 1) )
#define  BYTE2(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 2) )
#define  BYTE3(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 3) )

#define SCALE 10000.0f
serial::Serial ser; //声明串口对象 
float current_pos[3]={0.0f,0.0f,0.0f};
float current_angle[3]={0.0f,0.0f,0.0f};
px4flow::Flow distance_acc_msg;


void send_t265_data(uint8_t receive_flag){
    int sum = 0;
    u_int8_t pkg_tmp[31];
    pkg_tmp[0] = 0xFE;
    pkg_tmp[1] = 0x22;
    for(int i = 0;i<3;i++){
        int send_pos = (int)(SCALE*current_pos[i]);
        sum += send_pos;
        int send_angle = (int)(SCALE*current_angle[i]);
        sum += send_angle;
        // send pos buffer
        pkg_tmp[2+i*4] = BYTE3(send_pos);
        pkg_tmp[3+i*4] = BYTE2(send_pos);
        pkg_tmp[4+i*4] = BYTE1(send_pos);
        pkg_tmp[5+i*4] = BYTE0(send_pos);
        // send angle buffer
        pkg_tmp[14+i*4] = BYTE3(send_angle);
        pkg_tmp[15+i*4] = BYTE2(send_angle);
        pkg_tmp[16+i*4] = BYTE1(send_angle);
        pkg_tmp[17+i*4] = BYTE0(send_angle);
    }
    pkg_tmp[26] = BYTE3(sum);
    pkg_tmp[27] = BYTE2(sum);
    pkg_tmp[28] = BYTE1(sum);
    pkg_tmp[29] = BYTE0(sum);
    pkg_tmp[30] = receive_flag;
    ser.write(pkg_tmp,31);   
}
//回调函数 
void pose_callback(const nav_msgs::Odometry::ConstPtr& msg) 
{ 
    // ROS_INFO_STREAM("Writing to serial port" << msg->data); 
    current_pos[0] = msg->pose.pose.position.x;
    current_pos[1] = msg->pose.pose.position.y;
    current_pos[2] = msg->pose.pose.position.z;
    float w1,x1,y1,z1;
    x1 = msg->pose.pose.orientation.x;
    y1 = msg->pose.pose.orientation.y;
    z1 = msg->pose.pose.orientation.z;
    w1 = msg->pose.pose.orientation.w;
    current_angle[0] = (float)atan2(2*(w1*x1+y1*z1),1-2*(x1*x1+y1*y1));
    current_angle[1] = (float)asin(2*(w1*y1-x1*z1));
    current_angle[2] = (float)atan2(2*(w1*z1+x1*y1),1-2*(z1*z1+y1*y1));
}

void decode_data(ros::Publisher distance_acc_pub) 
{ 
    u_int8_t data_check;
    u_int8_t data_buffer[21];
    bool data_received = false;
    int distance_s;
    int accx_s;
    int accy_s;
    int yaw_s;
    int sum_check;
    bool check_status;
    for(int i = 1;i<10;i++){
        ser.read(&data_check,1);
        if(data_check==(u_char)(0xFE)){
            ser.read(&data_check,1);
            if(data_check==0x22){
                data_received = true;
                break;
            }
        }
    }
    if(data_received){
        ser.read(data_buffer,21);
        BYTE3(distance_s) = data_buffer[0];
        BYTE2(distance_s) = data_buffer[1];
        BYTE1(distance_s) = data_buffer[2];
        BYTE0(distance_s) = data_buffer[3];
        BYTE3(accx_s) = data_buffer[4];
        BYTE2(accx_s) = data_buffer[5];
        BYTE1(accx_s) = data_buffer[6];
        BYTE0(accx_s) = data_buffer[7];
        BYTE3(accy_s) = data_buffer[8];
        BYTE2(accy_s) = data_buffer[9];
        BYTE1(accy_s) = data_buffer[10];
        BYTE0(accy_s) = data_buffer[11];
        BYTE3(yaw_s) = data_buffer[12];
        BYTE2(yaw_s) = data_buffer[13];
        BYTE1(yaw_s) = data_buffer[14];
        BYTE0(yaw_s) = data_buffer[15];
        BYTE3(sum_check) = data_buffer[16];
        BYTE2(sum_check) = data_buffer[17];
        BYTE1(sum_check) = data_buffer[18];
        BYTE0(sum_check) = data_buffer[19];
    }
    check_status = (sum_check == distance_s+accx_s+accy_s+yaw_s);
    if(check_status){
        distance_acc_msg.header.stamp = ros::Time::now ();
        distance_acc_msg.hight = (float)distance_s/SCALE;
        distance_acc_msg.pos[0] = (float)accx_s/SCALE;
        distance_acc_msg.pos[1] = (float)accy_s/SCALE;
        distance_acc_msg.pos[2] = (float)yaw_s/SCALE;
        distance_acc_pub.publish(distance_acc_msg);
    }
    ROS_INFO("-------serial2pixhawk------\n"
    "hight:[\t%2.4f],accx:[\t%2.4f],accy:[\t%2.4f],\n"
    "yaw:[\t%2.4f],check[%d]\n"
    "======================================================\n",
    distance_acc_msg.hight,
    distance_acc_msg.pos[0],
    distance_acc_msg.pos[1],
    distance_acc_msg.pos[2],
    (int)(check_status)
    );
}

int main (int argc, char** argv) 
{ 
    ros::init(argc, argv, "Realsense2Serial"); 
    ros::NodeHandle nh; 
    ros::Subscriber pose_sub = nh.subscribe("/fused_odom", 1, pose_callback); 
    ros::Publisher distance_acc_pub = nh.advertise<px4flow::Flow>("/DistanceAcc", 1); 
    ros::Rate loop_rate(40); 
    try 
    { 
        //设置串口属性，并打开串口
        ser.setPort("/dev/ttyS0");
        ser.setBaudrate(19200);
        serial::Timeout to = serial::Timeout::simpleTimeout(500); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    {
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }
    int receive_status = 0;
    u_char receive_flag; 

    // ros::Rate loop_rate(150); 
    while(ros::ok()) 
    {
        switch (receive_status)
        {
        // don't receive
        case 0:
            receive_flag = 0x14;
            send_t265_data(receive_flag);
            break;
        // after send need to receive
        case 1:
            receive_flag = 0x15;
            send_t265_data(receive_flag);
            decode_data(distance_acc_pub);
            break;
        default:
            break;
        }
        receive_status += 1;
        if(receive_status==2){receive_status = 0;}
        ros::spinOnce();
        loop_rate.sleep();
    } 
}